#include "Compass.h"
#include <WProgram.h>

int _heading = 0;
int offset = 0;

void getHeading();

unsigned int Compass::heading() {
  getHeading();
  return (_heading + offset) % 3600;
}

void Compass::reset(unsigned int gpsHeading) {
  offset = 3600 + gpsHeading - _heading;
}
void compass(char command) {
  Wire.beginTransmission(I2C_ADDRESS);
  Wire.send(command);
  Wire.endTransmission();
}

void Compass::startCalibration() {
  compass(START_CALIBRATION);
}

void Compass::exitCalibration() {
  compass(EXIT_CALIBRATION);
  delay(1000);
}

void getHeading() {
  Wire.requestFrom(I2C_ADDRESS, 2);
  if (2 <= Wire.available()) {
    _heading = Wire.receive();
    _heading = _heading << 8;
    _heading += Wire.receive();
  }
}

void Compass::initialize() {
  Wire.begin();
  Wire.beginTransmission(I2C_ADDRESS);
  Wire.send(RESET);
  Wire.endTransmission();
  delay(500);
  setOperationMode();
}

void Compass::setOperationMode() {
  Wire.beginTransmission(I2C_ADDRESS);
  uint8_t buffer[3] = { WRITE_RAM, OPERATION_MODE_ADDRESS, CONTINUOUS_MODE_20HZ };
  Wire.send(buffer, 3);
  Wire.endTransmission();
  delay(10);
  compass(GET_HEADING);
  delay(10);
}
